#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S4,     angle,          sensorI2CCustomFastSkipStates)
#pragma config(Motor,  motorA,          C1,            tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorB,          C2,            tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorC,          C3,            tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     FL,            tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     FR,            tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     RL,            tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     RR,            tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     Arm,           tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C2_2,     Lift,          tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    SpinL,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    SpinR,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    GrabL,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    GrabR,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    Lift2,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    Lift1,                tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#include "include.h"



void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  servo[SpinL]=128;
  servo[SpinR]=128;
  servo[GrabL]=255;
  servo[GrabR]=45;
  servo[Lift1]=230;
  servo[Lift2]=0;
  chooser();
  return;
}



int straightdistance[]={35000/2,4200/2,4800/2,2000/2};
int straightspeed[]={60,60,-60,60};
int turndistance[]={2800/2,123/2,123/2,123/2};
int turnspeed[]={-70,100,100,100};

task main()
{

  initializeRobot();

  waitForStart(); // Wait for the beginning of autonomous phase.

  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
  ////                                                   ////
  ////    Add your robot specific autonomous code here.  ////
  ////                                                   ////
  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
  wait1Msec(1000*waittime);
  straight(straightspeed[0],straightdistance[0]);
  while(true)
  {}
}
